package com.arithmetic;

import android.util.Log;

import com.DataProcess.Robot;
import com.pid.PuduPID;
import com.pudu.json.Json2PC;
import com.sensors.Lidar;

/**
 * Created by tjpu on 2016/10/12.
 */
public class MotorsControl {

    static String thisTagLog = "MotorsControl";

    static double MAX_SAFE_SPEED = 1.0;
    static double OPENLOOP_SCALE = 60;     //开环时候的放缩比例，将m/s直接放大成PWM输出
    static double TARGET_ZERO_STOP = 0.02;
    static double CURRENT_ZERO_STOP = 0.08;  //当目标速度和当前速度同时小于ZERO_STOP,输出清零，防震荡
    static double CLOSE_MIN_SPEED = 0.05;
    double [] input = new double[2];
    double [] target = new double[2];
    double [] output = new double[2];
    double [] outputLast = new double[2];

    public static int PIDLoopPeriod = 1;
    public static PuduPID left = new PuduPID(Robot.P_speed,Robot.I_speed,Robot.D_speed, PIDLoopPeriod);
    public static PuduPID right = new PuduPID(Robot.P_speed,Robot.I_speed,Robot.D_speed, PIDLoopPeriod);

    double adjustStep = 0.1;
    double adjustK = 0;
    public double[] adjustTarget(double[] aim, boolean isSlowDown) {
        if(isSlowDown)
            adjustK -= adjustStep;
        else
            adjustK += adjustStep;

        if(adjustK > 1.0) adjustK = 1.0;
        if(adjustK < 0.0) adjustK = 0.0;
        if(!Robot.isEnable) adjustK = 0.0;

        double[] aimOut = new double[2];
        aimOut[0] = aim[0] * adjustK;
        aimOut[1] = aim[1] * adjustK;

        Log.i(thisTagLog,"adjustK: " + adjustK);
        return aimOut;
    }

    public void putTarget(double[] aim,double[] current,boolean isCloseLoop) {

        System.arraycopy(aim,0,target,0,2);   // 目标值
        System.arraycopy(current,0, input,0,2);  // 输入
        System.arraycopy(output,0,outputLast,0,2);
        if (isCloseLoop) {
            if (Math.abs(target[0]) < CLOSE_MIN_SPEED) {
                output[0] = outputLast[0] * 0.2;   //后续改成刹车指令
                left.cleanPID();
                Log.i(thisTagLog,"slow to open loop");
            } else {
                output[0] = left.calculatePid(target[0],input[0]);
            }

            if (Math.abs(target[1]) < CLOSE_MIN_SPEED) {
                output[1] = outputLast[1] * 0.2;
                right.cleanPID();
            } else {
                output[1] = right.calculatePid(target[1],input[1]);
            }
        } else {
            output[0] = target[0] * OPENLOOP_SCALE;
            output[1] = target[1] * OPENLOOP_SCALE;
            cleanPID();
        }

        check();

        Json2PC.sendPID(input[0], input[1], target[0], target[1]);
    }

    public double[] adjustOut() {
        double [] out = new double[2];

        return out;
    }

    public double[] getOutput() {

        return output;
    }

    public void cleanPID() {
        left.cleanPID();
        right.cleanPID();
    }

    void check() {
        if (Math.abs(target[0]) < TARGET_ZERO_STOP && Math.abs(input[0]) < CURRENT_ZERO_STOP) {
            output[0] = 0.0;
            left.cleanPID();
        }
        if (Math.abs(target[1]) < TARGET_ZERO_STOP && Math.abs(input[1]) < CURRENT_ZERO_STOP) {
            output[1] = 0.0;
            right.cleanPID();
        }

        if (Math.max(input[0], input[1]) > MAX_SAFE_SPEED) {
            output[0] = 0.0;
            output[1] = 0.0;
            cleanPID();
            Robot.isEnable = false;
        }

//        if(Robot.isObstacle)
    }
}
